/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2021 STMicroelectronics.
  * All rights reserved.</center></h2>
  *
  * This software component is licensed by ST under Ultimate Liberty license
  * SLA0044, the "License"; You may not use this file except in compliance with
  * the License. You may obtain a copy of the License at:
  *                             www.st.com/SLA0044
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "oslib.h"
#include "boxReader.h"
#include "laser.h"
#include "shooter.h"
#include "rudder_chassis.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define ADC1_BUFFER_SIZE (8 * 2)
// #define ADC2_BUFFER_SIZE  (8*2)
// #define ADC3_BUFFER_SIZE  (8*2)
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
static uint16_t adc1_buffer[ADC1_BUFFER_SIZE];
// static uint16_t adc2_buffer[ADC2_BUFFER_SIZE];
// static uint16_t adc3_buffer[ADC2_BUFFER_SIZE];

osSemaphoreId_t TripodTokenSema;
laser_init_t shooter_laser_attrs = {
    .adc_buffer = adc1_buffer,
    .adc_buffer_size = ADC1_BUFFER_SIZE,
    .adc_split_count = 2,
    .adc_split_offset = 0,
    .k_param = 1.1204,
    .b_param = 2947.3,
};
laser_init_t chassis_laser1_attrs = {
    .adc_buffer = adc1_buffer,
    .adc_buffer_size = ADC1_BUFFER_SIZE,
    .adc_split_count = 2,
    .adc_split_offset = 1,
    .k_param = 0.3991,
    .b_param = 390.83,
};
shooter_init_t shooter1_attrs = {
    .clutch_port = CLUTCH1_GPIO_Port,
    .clutch_pin = CLUTCH1_Pin,
    .motor_hcan = &hcan2,
    .dji_board_id = 1,
    .dji_fmotor_id = 3,
    .dji_pmotor_id = 2,
    .dji_ymotor_id = 1,
};
/* USER CODE END Variables */
/* Definitions for defaultTask */
osThreadId_t defaultTaskHandle;
const osThreadAttr_t defaultTask_attributes = {
  .name = "defaultTask",
  .priority = (osPriority_t) osPriorityNormal,
  .stack_size = 128 * 4
};
/* Definitions for boxReader */
osThreadId_t boxReaderHandle;
const osThreadAttr_t boxReader_attributes = {
  .name = "boxReader",
  .priority = (osPriority_t) osPriorityNormal2,
  .stack_size = 192 * 4
};
/* Definitions for tripodHead */
osThreadId_t tripodHeadHandle;
const osThreadAttr_t tripodHead_attributes = {
  .name = "tripodHead",
  .priority = (osPriority_t) osPriorityLow,
  .stack_size = 192 * 4
};
/* Definitions for EncoderEx */
osThreadId_t EncoderExHandle;
const osThreadAttr_t EncoderEx_attributes = {
  .name = "EncoderEx",
  .priority = (osPriority_t) osPriorityAboveNormal2,
  .stack_size = 128 * 4
};
/* Definitions for TakeArrowTask */
osThreadId_t TakeArrowTaskHandle;
const osThreadAttr_t TakeArrowTask_attributes = {
  .name = "TakeArrowTask",
  .priority = (osPriority_t) osPriorityAboveNormal3,
  .stack_size = 192 * 4
};
/* Definitions for CtrlBtnTask */
osThreadId_t CtrlBtnTaskHandle;
const osThreadAttr_t CtrlBtnTask_attributes = {
  .name = "CtrlBtnTask",
  .priority = (osPriority_t) osPriorityHigh,
  .stack_size = 192 * 4
};
/* Definitions for ChassisTask */
osThreadId_t ChassisTaskHandle;
const osThreadAttr_t ChassisTask_attributes = {
  .name = "ChassisTask",
  .priority = (osPriority_t) osPriorityLow3,
  .stack_size = 256 * 4
};
/* Definitions for RaceTask */
osThreadId_t RaceTaskHandle;
const osThreadAttr_t RaceTask_attributes = {
  .name = "RaceTask",
  .priority = (osPriority_t) osPriorityLow3,
  .stack_size = 128 * 4
};
/* Definitions for LaserTask */
osThreadId_t LaserTaskHandle;
const osThreadAttr_t LaserTask_attributes = {
  .name = "LaserTask",
  .priority = (osPriority_t) osPriorityLow,
  .stack_size = 192 * 4
};
/* Definitions for LightTask */
osThreadId_t LightTaskHandle;
const osThreadAttr_t LightTask_attributes = {
  .name = "LightTask",
  .priority = (osPriority_t) osPriorityNormal1,
  .stack_size = 128 * 4
};
/* Definitions for EncoderExQueue */
osMessageQueueId_t EncoderExQueueHandle;
const osMessageQueueAttr_t EncoderExQueue_attributes = {
  .name = "EncoderExQueue"
};
/* Definitions for CtrlBtnQueue */
osMessageQueueId_t CtrlBtnQueueHandle;
const osMessageQueueAttr_t CtrlBtnQueue_attributes = {
  .name = "CtrlBtnQueue"
};
/* Definitions for EventTakeArrow */
osEventFlagsId_t EventTakeArrowHandle;
const osEventFlagsAttr_t EventTakeArrow_attributes = {
  .name = "EventTakeArrow"
};
/* Definitions for EventRaceStateTrans */
osEventFlagsId_t EventRaceStateTransHandle;
const osEventFlagsAttr_t EventRaceStateTrans_attributes = {
  .name = "EventRaceStateTrans"
};

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

/* USER CODE END FunctionPrototypes */

void StartDefaultTask(void *argument);
extern void boxReaderFunc(void *argument);
extern void tripodHeadFunc(void *argument);
extern void shooter_encoder_ex_task(void *argument);
extern void GA_task(void *argument);
extern void ctrl_btn_task(void *argument);
void ChassisTaskFunc(void *argument);
extern void RaceTaskFunc(void *argument);
extern void laser_task(void *argument);
extern void lb_task(void *argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */
  HAL_ADC_Start_DMA(&hadc1, (uint32_t *)adc1_buffer, ADC1_BUFFER_SIZE);
  OSLIB_Init();
  laser_init(shooter_laser, &shooter_laser_attrs);
  laser_init(chassis_laser1, &chassis_laser1_attrs);
  shooter1_attrs.laser = shooter_laser;
  shooter_init(shooter1, &shooter1_attrs);
  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */

  /* USER CODE END RTOS_MUTEX */

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  // AimInitSema = osSemaphoreNew(1, 0, &AimInitSema_Attributes);
  TripodTokenSema = osSemaphoreNew(1, 0, NULL);
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* Create the queue(s) */
  /* creation of EncoderExQueue */
  EncoderExQueueHandle = osMessageQueueNew (8, sizeof(CAN_ConnMessage), &EncoderExQueue_attributes);

  /* creation of CtrlBtnQueue */
  CtrlBtnQueueHandle = osMessageQueueNew (8, sizeof(CAN_ConnMessage), &CtrlBtnQueue_attributes);

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */

  /* Create the thread(s) */
  /* creation of defaultTask */
  defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);

  /* creation of boxReader */
  boxReaderHandle = osThreadNew(boxReaderFunc, NULL, &boxReader_attributes);

  /* creation of tripodHead */
  tripodHeadHandle = osThreadNew(tripodHeadFunc, NULL, &tripodHead_attributes);

  /* creation of EncoderEx */
  EncoderExHandle = osThreadNew(shooter_encoder_ex_task, NULL, &EncoderEx_attributes);

  /* creation of TakeArrowTask */
  TakeArrowTaskHandle = osThreadNew(GA_task, NULL, &TakeArrowTask_attributes);

  /* creation of CtrlBtnTask */
  CtrlBtnTaskHandle = osThreadNew(ctrl_btn_task, NULL, &CtrlBtnTask_attributes);

  /* creation of ChassisTask */
  ChassisTaskHandle = osThreadNew(ChassisTaskFunc, NULL, &ChassisTask_attributes);

  /* creation of RaceTask */
  RaceTaskHandle = osThreadNew(RaceTaskFunc, NULL, &RaceTask_attributes);

  /* creation of LaserTask */
  LaserTaskHandle = osThreadNew(laser_task, NULL, &LaserTask_attributes);

  /* creation of LightTask */
  LightTaskHandle = osThreadNew(lb_task, NULL, &LightTask_attributes);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */

  /* USER CODE END RTOS_THREADS */

  /* Create the event(s) */
  /* creation of EventTakeArrow */
  EventTakeArrowHandle = osEventFlagsNew(&EventTakeArrow_attributes);

  /* creation of EventRaceStateTrans */
  EventRaceStateTransHandle = osEventFlagsNew(&EventRaceStateTrans_attributes);

  /* USER CODE BEGIN RTOS_EVENTS */
  /* add events, ... */
  /* USER CODE END RTOS_EVENTS */

}

/* USER CODE BEGIN Header_StartDefaultTask */
/**
  * @brief  Function implementing the defaultTask thread.
  * @param  argument: Not used 
  * @retval None
  */
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void *argument)
{
  /* USER CODE BEGIN StartDefaultTask */
  /* Infinite loop */
  for (;;)
  {
    HAL_GPIO_TogglePin(LED2_GPIO_Port, LED2_Pin);
    osDelay(500);
  }
  // osThreadExit();
  /* USER CODE END StartDefaultTask */
}

/* USER CODE BEGIN Header_ChassisTaskFunc */
/**
* @brief Function implementing the ChassisTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_ChassisTaskFunc */
void ChassisTaskFunc(void *argument)
{
  /* USER CODE BEGIN ChassisTaskFunc */
  RudderChassis_Init();
  Handle_Init();
  HAL_GPIO_WritePin(Relay_GPIO_Port, Relay_Pin, RELAY_ACCESS);
  Chassis_RemappingPathSetsSpeed(points_pos1, points_pos1_num, 1.0f / 68.0f, 0, 0.08, 2.8);
  Chassis_RemappingPathSetsSpeed(points_pos2, points_pos2_num, 1.0f / 100.0f, 0, 0.08, 1.2);
  //Chassis_RemappingPathSetsSpeed(points_pos3, 41, 1.0f / 50.0f, 0, 0.25, 1.5);
  //Chassis_RemappingPathSetsSpeed(points_pos4, 41, 1.0f / 50.0f, 0, 0.25, 1.5);
  //Chassis_RemappingPathSetsSpeed(points_pos5, 41, 1.0f / 50.0f, 0, 0.25, 1.5);
  static uint32_t last_time;
  last_time = osKernelGetTickCount();
  /* Infinite loop */
  for (;;)
  {
    RudderChassis_Exe();

    Chassis_PrintTargetStatus();
    Chassis_PrintPostureStatus();
    SW_PrintMotorStatus();

    uint32_t now_time = osKernelGetTickCount();
    if (now_time - last_time > 10)
    {
      RudderChassis.SteerMotors.can_send_flag = 1;
      RudderChassis.DriveMotors.can_send_flag = 1;
      last_time = now_time;
    }
    TimeFlag_5ms++;
    if (TimeFlag_5ms % 4 == 0)
    {
      TimeFlag_20ms = 1;
    }
    else
    {
      TimeFlag_20ms = 0;
    }
    if (TimeFlag_5ms % 10 == 0)
    {
      TimeFlag_50ms = 1;
    }
    else
    {
      TimeFlag_50ms = 0;
    }
    if (TimeFlag_5ms % 20 == 0)
    {
      TimeFlag_100ms = 1;
    }
    else
    {
      TimeFlag_100ms = 0;
    }
    osDelay(5);
  }
  /* USER CODE END ChassisTaskFunc */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */

/* USER CODE END Application */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
